#include <math.h>
#include "Transformation.h"

namespace opennav {

Transformation::Transformation() :
		m00(0), m01(0),
		m10(0), m11(0),
		scale_(1.0f),
		rotation_(1.0f),
		shift_(0) { }

void Transformation::set_rotation(float rotation) {
	rotation_ = rotation;
}

float Transformation::rotation() const {
	return rotation_;
}

void Transformation::set_scale(float scale) {
	scale_ = scale;
}

float Transformation::scale() const {
	return scale_;
}

void Transformation::set_map_center(Coordinate& coordinate) {
	map_center_ = coordinate;
}

Coordinate Transformation::map_center() const {
	return map_center_;
}

void Transformation::set_screen_center(Point& point) {
	screen_center_ = point;
}

Point Transformation::screen_center() const {
	return screen_center_;
}

void Transformation::setup() {
	float angle = rotation_ * M_PI / 180.0;
    float rsin = sinf(angle);
    float rcos = cosf(angle);
    shift_ = log2f(scale_) + SHIFT;
    float fac = (float)(1 << shift_) / scale_;
    m00 = roundf(fac * rcos);
    m10 = roundf(-fac * rsin);
    m01 = roundf(fac * rsin);
    m11 = roundf(fac * rcos);
    double det = (double)m00 * (double)m11 - (double)m01 * (double)m10;
    im00 = (double)m11 / det;
    im10 = (double)-m01 / det;
    im01 = (double)-m10 / det;
    im11 = (double)m00 / det;
}

Point Transformation::transform(const opennav::Coordinate& coordinate) const {
    int32_t a1 = coordinate.longitude - map_center_.longitude;
    int32_t a2 = coordinate.latitude - map_center_.latitude;
    int64_t b1 = m00 * a1 + m10 * a2;
    int64_t b2 = m01 * a1 + m11 * a2;
    int32_t x = (b1 >> shift_) + screen_center_.x;
    int32_t y = screen_center_.y - (b2 >> shift_);
    return Point(x, y);
}

Coordinate Transformation::inv_transform(const opennav::Point& point) const {
	int64_t a1 = (point.x - screen_center_.x) << shift_;
	int64_t a2 = (screen_center_.y - point.y) << shift_;
	int64_t b1 = round(im00 * (double)a1 + im01 * (double)a2);
	int64_t b2 = round(im10 * (double)a1 + im11 * (double)a2);
	return Coordinate((int32_t)(b1 + map_center_.longitude), (int32_t)(b2 + map_center_.latitude));
}

void Transformation::transform(const std::vector<Coordinate>& coordinates, std::vector<Point>& points) const {
	for (std::vector<opennav::Coordinate>::const_iterator it = coordinates.begin(); it != coordinates.end(); it++) {
		points.push_back(transform(*it));
	}
}

}
